#include "encoders.h"
#include <avr/interrupt.h>
//TODO: ES necesario el uso de cli() y sei()?? revisar esto con el datasheet.
//Solo retorna el valor de las cuentas acumuladas. El calculo de velocidad se debe realizar en las rutinas de MID




static int posCountsL;
static int posCountsR;
static unsigned char motorLeftDir;
static unsigned char motorRightDir;

//devuelven las cuentas de los encoders. Si la rueda gira hacia atras, las cuentas disminuyen. Si es hacia adelante, aumentan.
//
unsigned int encoders_readLeft(void){
	
	//Reading a 16-bit register should be an atomic operation. Thats why we disable interrupts when reading TCNTn.
	cli();
	unsigned int temp = TCNT1;
	sei();
	
	if(motorLeftDir == ENC_REVERSE)
		temp = posCountsL - temp;
	
	return temp;
}

//Solo retorna el valor de las cuentas acumuladas. El calculo de velocidad se debe realizar en las rutinas de MID
unsigned int encoders_readRight(void){

	//atomic operation
	cli();
	unsigned int temp = TCNT3;
	sei();
	if(motorRightDir == ENC_REVERSE)
		temp = posCountsR - temp;
	
	return temp;
}

//Funcion que permite devolver los dos valores al tiempo
void encoders_readAll(unsigned int *enc[]){

	*enc[0]=encoders_readLeft();
	*enc[1]= encoders_readRight();

}
unsigned int encoders_read(unsigned char enc){
	if(enc == ENC_LEFT)
		return encoders_readLeft();
	else if (enc == ENC_RIGHT)
		return  encoders_readRight();
	else
		return 0;
}

//Configura encoders, primero los pines, luego los timers, en donde se configuran en modo free running, es decir, cuenta
//Hasta el tope, lo que se controla es la fuente de reloj, que será tomada de los pines externos T1 y T3
//La funcion confiEncoders por defecto deja los encoders en modo Stop, hay que arrancarlos con startEncoders
void encoders_init(void){

	//Port Configuration as Input
	
	//Cambiar nombres de los puertos, para que el codigo sea mas legible
	DDRD &= ~(1<<PD6);
	DDRE &= ~(1<<PE7);
	
	//Normal Mode
	TCCR1A=0;
	TCCR3A=0;

	//Timers in stop mode, no clock source
	TCCR1B = 0; 
	TCCR3B = 0; 
	//Timer Overflow Interrupt enabled
	//TIMSK1 |= (1<<TOIE1);
	//TIMSK3 |= (1<<TOIE3);


}
//Rutina para arrancar encoders, define la fuente de reloj de los pines T1 y T3
void encoders_start(void){
	
	//Clock Source is T Pin on rising Edge
	TCCR1B = (1<<CS12) | (1<<CS11) | (1<<CS10);
	TCCR3B = (1<<CS32) | (1<<CS31) | (1<<CS30);


}
//Rutina para detener encoders, quita la fuente de reloj
void encoders_stop(void){
	//Timers in stop mode, no clock source
	TCCR1B = 0; 
	TCCR3B = 0; 

}

void encoders_writeLeft(unsigned int val)
{
	cli();
	TCNT1 = val;
	sei();
}

void encoders_writeRight(unsigned int val)
{
  //atomic operation
	cli();
	TCNT3 = val;
	sei();	
}

void encoders_write(unsigned int left,unsigned int right)
{
	//atomic operation
	cli();
	TCNT1 = left;
	TCNT3 = right;
	sei();	
	
	//TODO: Revisar, estas modificaciones no se sabe si son correctas, se hicieron para garantizar cero en la posicion cuando se hiciera 
	//un resetOdometry desde el Player.
	posCountsL = 0;
	posCountsR = 0;
	motorLeftDir = ENC_FORWARD;
	motorRightDir = ENC_REVERSE;
	//Hasta aqui llegan las mods
	
}
//TODO: No deberia ser una rutina que fuera de motores, ya que estos son los uqe manejan la direccion?
void encoders_setDirection(unsigned char left, unsigned char right)
{
	
	if(motorLeftDir != left)
	{
		
		if(left == ENC_REVERSE)
		{
			posCountsL = encoders_readLeft();
			encoders_resetLeft();
		}
		else
		{
			
			encoders_writeLeft(encoders_readLeft());
			posCountsL = 0;
		}
		
		motorLeftDir = left;
	}
	if(motorRightDir != right)
	{	
		if(right == ENC_REVERSE)
		{
			posCountsR = encoders_readRight();
			encoders_resetRight();
		}
		else
		{
			encoders_writeRight(encoders_readRight());
			posCountsR = 0;
		}
		
		motorRightDir = right;
	}
	
	
	
	
	
}
